
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.util.ArrayList;
import lejos.nxt.*;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.RConsole;
import lejos.robotics.navigation.TachoPilot;


public class BarCodeFSMThread{

	static LightSensor sensor = new LightSensor(SensorPort.S1);
	//static final TachoPilot pilot = new TachoPilot(2.184f, 4.233f, Motor.A, Motor.C, true);
	// samplingPeriod in milliseconds
	static final int samplingPeriod = 10;
	static final int numBufferSamples = 3;

	int[] signalBuffer;
	int[] barcode = new int[7];
	static ArrayList<Integer> stateDurations = new ArrayList<Integer>();
	static boolean stopMe;

	public boolean readingStrip;
	private Master master;
	static int state;
	static int prevState;
	static int BROWN = 508;
	static int WHITE = 459;
	static int BLACK = 684;
	static int EDGE = 0;
	static int Vbz;
	static int Vwb;
	private static BTConnection connection;
	private static DataInputStream dataIn;
	private static DataOutputStream dataOut;


	public BarCodeFSMThread(Master master) {
		// roep constructor van superclass aan ( Thread() )
		super();
		this.master = master;
		signalBuffer = new int[numBufferSamples];
		readingStrip = false;
		state = BROWN;

		//BarCodeFSMThread reader = new BarCodeFSMThread();
		BarCodeFSMThread.sensor.setFloodlight(true);
		//		connect();
					//RConsole.openBluetooth(0);

		System.out.println("Press ENTER to calibrate BLACK");
		Button.ENTER.waitForPressAndRelease();
		BLACK = SensorPort.S1.readRawValue();
		System.out.println(" "+ BLACK);


		System.out.println("Press ENTER to calibrate WHITE");
		Button.ENTER.waitForPressAndRelease();
		WHITE = SensorPort.S1.readRawValue();
		System.out.println(" "+ WHITE);

		System.out.println("Press ENTER to calibrate BROWN");
		Button.ENTER.waitForPressAndRelease();
		BROWN = SensorPort.S1.readRawValue();
		System.out.println(" "+ BROWN);

		//			Vbz = (BROWN + BLACK) / 2;
		//			Vwb = (WHITE + BROWN) / 2;
		Vbz = BROWN+(BLACK - BROWN) / 5;
		Vwb = BROWN+ (WHITE - BROWN) / 3;

		RConsole.println("Press ENTER to start driving");
		Button.ENTER.waitForPressAndRelease();
		LCD.clear();
		//pilot.forward();
		fillSignalBuffer(BROWN);
		state = BROWN;

	}
	
	public void start(){
		Thread barCodeReader = new Thread(new BarcodeThread());
		barCodeReader.start();
	}

	//int starttime=0;
	class BarcodeThread implements Runnable {
		public void run(){
			float initialDistance = 0;
			for (;;) {
//				//RConsole.println(System.currentTimeMillis()-starttime);
//				starttime=System.currentTimeMillis();
				try {
					Thread.sleep(samplingPeriod);
				} catch (InterruptedException e) {
				}
				int lightValue = SensorPort.S1.readRawValue();	
				push(lightValue);
				int gradient = signalBuffer[signalBuffer.length - 1] - signalBuffer[signalBuffer.length - 2];

				boolean onEdge = gradient >= 18 || gradient <= -18;
				boolean lightBrown = (lightValue <= Vbz) && (lightValue > Vwb);
				boolean lightWhite = (lightValue <= Vwb);
				boolean lightBlack = (lightValue > Vbz);
				
				// later evt. gebruiken om te bepalen of het echte barcode is
				

				// switch statement werkt niet want BROWN,... zijn geen constanten
				prevState = state;

				RConsole.println(" "+ state);

				if (state == BROWN) {
					if (onEdge || lightBlack || lightWhite) {
						// start reading the barcode
						readingStrip = true;
						initialDistance = master.getTravelDistance();
						System.out.println("Forward");
						master.driveForward();
						state = EDGE;
					} else {
						readingStrip = false;
						state = BROWN;
					}
				} else if (state == WHITE) {
					if (onEdge || lightValue > Vwb) {
						state = EDGE;
					} else {
						readingStrip = true;
						state = WHITE;
					}
				} else if (state == BLACK) {
					if (onEdge || lightValue < Vbz) {
						state = EDGE;
					} else {
						readingStrip = true;
						state = BLACK;
					}
				} else if (state == EDGE) {
					if (!onEdge && lightBrown) {
						// leaving a barcode - stop reading
						RConsole.println("van barcode gereden");

						readingStrip = false;
						// interpreteer ingelezen waarden
						if(stateDurations.size()>=2 && (master.getTravelDistance() - initialDistance)>12){
							// TODO: rekening houden met mogelijke overflow
							//System.out.println("start interpret");

							RConsole.println("start interpret");

							interpretBarcode(master.getTravelDistance() - initialDistance);
						}
						// clean up the old ArrayList
						stateDurations = new ArrayList<Integer>();
						state = BROWN;
					} else if (!onEdge && lightWhite) {
						readingStrip = true;
						state = WHITE;
					} else if (!onEdge && lightBlack) {
						readingStrip = true;
						state = BLACK;
					} else {
						state = EDGE;
					}
				}

				// om grafief te tekenen
				//				try {
				//					dataOut.writeInt(lightValue);
				//					dataOut.writeInt(gradient);
				//					dataOut.writeInt(state);
				//					dataOut.flush();
				//				} catch (IOException e) {
				//			
				//				}

				if(readingStrip) {
					if (state == prevState) {
						int lastElement = stateDurations.get(stateDurations.size() - 1);
						stateDurations.remove(stateDurations.size() - 1);
						stateDurations.add(lastElement - 1);
					} else {
						stateDurations.add(state);
						stateDurations.add(-1);
					}
				}
				// Don't Stop Me Now!!!!
				// Don't Stop Me Now!!!!
				// Cause i'm having a good time, having a good time...
				if(stopMe)	break;
			}
		}
	}

	private void push(int lightValue)
	{
		for (int i = 0; i < signalBuffer.length-1; i++) {
			signalBuffer[i] = signalBuffer[i + 1];
		}
		signalBuffer[signalBuffer.length-1] = lightValue;
		// Low pass filter: zorgt voor langere reactietijd
		//signalBuffer[signalBuffer.length-1] = (int) ((0.25 * lightValue) + (1 - 0.25) * signalBuffer[signalBuffer.length-1]);
	}

	private void interpretBarcode(float barcodeDistance)
	{
		boolean infiniteLoop=false;
		for (int i = 0; i < stateDurations.size(); i++) {
			RConsole.println("stateDurations("+i+") : " + stateDurations.get(i));	
		}
		RConsole.println("EDGES VERWIJDEREN:");
		// ============== EDGE states verwijderen en totale duration van barcode bepalen ====================
		int duration = 0;
		ArrayList<Integer> tempstateDurations = new ArrayList<Integer>();
		for (int i = 0; i < stateDurations.size(); i++) {
			//RConsole.println("working on " + i + " = " + stateDurations.get(i));
			int curState = stateDurations.get(i);
			if ( curState > 0) {
				// negatieve durations !!!
				duration -= stateDurations.get(i + 1);
			} else if (curState == EDGE) {
				int edgeDur = -stateDurations.get(i + 1);
				int lower = edgeDur / 2;
				int upper = edgeDur - lower;
				if (i - 1 >= 0) {
					int updatedDurPrev = stateDurations.get(i - 1) - lower;
					stateDurations.set(i - 1, updatedDurPrev);
				}
				if (i + 3 < stateDurations.size()) {
					int updatedDurNext = stateDurations.get(i + 3) - upper;
					stateDurations.set(i + 3, updatedDurNext);
				}
				//want was er nog niet bijgeteld, upper wordt later bijgeteld:
				duration += lower;
			}
		}
		for (int i = 0; i < stateDurations.size(); i+=2) {
			if(stateDurations.get(i)!= EDGE) {
				tempstateDurations.add(stateDurations.get(i));
				tempstateDurations.add(stateDurations.get(i+1));
			}
		}
		stateDurations = tempstateDurations;
		for (int i = 0; i < stateDurations.size(); i++) {
			RConsole.println("stateDurations("+i+") : " + stateDurations.get(i));	
		}
		RConsole.println("duration = " + duration);
		float barDuration = duration / 7;
		RConsole.println("barduration = " + barDuration);
		
		// ============== streepjesLengte toewijzen aan elke state WHITE/BLACK ==================-
		int[][] stateAttribs = new int[7][3];
		int arrIndex = 0;
		for (int i = 0; i < stateDurations.size() && arrIndex < 7; i++) {
			int curState = stateDurations.get(i);
			if (curState > 0) {
				// echte lengte van deze state
				// als curState > 0 is er altijd een volgend element (nl de duration)
				int stateDur = -stateDurations.get(i + 1);
				// verschil tussen toegewezen streepjeslengte en echte lengte
				int rem = duration;
				// toegewezen streepjeslengte
				int assignedLength = 0;
				// vergelijk echte lengte met alle veelvouden van 1 gemiddelde streepjeslengte
				for (int j = 1; j <= 7; j++) {
					// currentRem < 0 betekent stateDur > j * barDuration
					// currentRem > 0 betekent stateDur < j * barDuration
					int currentRem = ((int) (j * barDuration)) - stateDur;
					if (Math.abs(currentRem) < Math.abs(rem)) {
						// verschil met toegewezen streepjeslengte
						rem = currentRem;
						// toegewezen streepjeslengte
						assignedLength = j;
					}
				}
				stateAttribs[arrIndex][0] = curState;
				stateAttribs[arrIndex][1] = rem;
				stateAttribs[arrIndex][2] = assignedLength;
				RConsole.println("Curstate/rem/assignedLength: " + curState + " " + rem + " " + assignedLength);
				arrIndex++;
			}
		}
		
		if (arrIndex <= 6) {
			//  maak state negatief om einde aan te geven
			stateAttribs[arrIndex][0] = -1;
		}
		
		// ================ check of totale streepjeslengte gelijk aan 7 en corrigeer indien nodig ==================
		int assignedLengthsSum = 0;
		for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
			assignedLengthsSum += stateAttribs[i][2];
		}
		if (assignedLengthsSum > 7) {
//			RConsole.println("groter dan 7");
			RConsole.println("groter dan 7");
			while (assignedLengthsSum != 7) {
				// zoek die state(index) met rem > 0 en grootste rem (dus echte lengte korter dan toegewezen en het minst dicht bij toegewezen lengte)
				int largestRem = 0;
				boolean found = false;
				int wantedStateIndex = 0;
				for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
					if (stateAttribs[i][1] > largestRem) {
						largestRem = stateAttribs[i][1];
						wantedStateIndex = i;
						found = true;
					}
				}
//				RConsole.println("wantedstateindex " + wantedStateIndex);
				if (found) {
					// maak toegewezen streepjeslengte 1 korter
					stateAttribs[wantedStateIndex][2] -= 1;
					// pas rem aan
					stateAttribs[wantedStateIndex][1] = (int) -(barDuration - largestRem);
					assignedLengthsSum = 0;
					for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
						assignedLengthsSum += stateAttribs[i][2];
					}
				} else {
					// maak de langste state 1 korter
					int longestBarLength = 0;
					wantedStateIndex = 0;
					found = false;
					for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
						if (stateAttribs[i][2] > longestBarLength) {
							wantedStateIndex = i;
							longestBarLength = stateAttribs[i][2];
						}
					}
					// maak toegewezen streepjeslengte 1 korter
					stateAttribs[wantedStateIndex][2] -= 1;
					assignedLengthsSum = 0;
					for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
						assignedLengthsSum += stateAttribs[i][2];
					}
				}
			}
		} else if (assignedLengthsSum < 7) {
			//RConsole.println("kleiner dan 7");
			//System.out.println("kleiner dan 7");
			int k=0;
//			RConsole.println("kleiner dan 7");
			RConsole.println("kleiner dan 7");
			while (assignedLengthsSum != 7) {
				k++;
				System.out.println("1");
				RConsole.println("1");

				if(k>200){
					infiniteLoop=true;
					break;
				}
					
			// zoek die state(index) met rem < 0 en kleinste rem (dus echte lengte langer dan toegewezen en het minst dicht bij toegewezen lengte)
				
				int smallestRem = 0;
				boolean found = false;
				int wantedStateIndex = 7;
				for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
					RConsole.println("komt in lus");

					if (stateAttribs[i][1] < smallestRem) {
						smallestRem = stateAttribs[i][1];
						wantedStateIndex = i;
						found = true;
					}
				}
			
				if (found) {
					// maak toegewezen streepjeslengte 1 langer
					stateAttribs[wantedStateIndex][2] += 1;
					// pas rem aan
					stateAttribs[wantedStateIndex][1] = (int) barDuration + smallestRem; // smallest rem negatief
					assignedLengthsSum = 0;
					for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
						assignedLengthsSum += stateAttribs[i][2];
					}
				} else {
					// maak de langste state 1 langer
					int longestBarLength = 0;
					wantedStateIndex = 0;
					found = false;
					//HIER FOUT
					for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
						//RConsole.println(stateAttribs[i][2]);
						if (stateAttribs[i][2] > longestBarLength) {
							wantedStateIndex = i;
							longestBarLength = stateAttribs[i][2];
						}
					}
					// maak toegewezen streepjeslengte 1 korter
					stateAttribs[wantedStateIndex][2] += 1;
					assignedLengthsSum = 0;
					for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
						assignedLengthsSum += stateAttribs[i][2];
					}
				}
			}
		}

		if(!infiniteLoop){
		// k telt aantal streepjes van streepjescode
		int k = 0;
		RConsole.println("begin lus");
		for (int i = 0; k < 7;i++) {
			//System.out.println("binnenstelus");

			for (int j = 0; j<stateAttribs[i][2]; j++) {
				barcode[k]=stateAttribs[i][0]==BLACK? 1 : 0;
				k++;
			}
		}
		
		//if (barcodeDistance > 0.8f) {
			sendBarCode(correctBarCode(barcode));
		//}
		}
		else{
			System.out.println("infinite loop!");
		}
	}
	
	private void sendBarCode(int[] barcode){
		//RConsole.println("The Barcode is: " + barcode[0] + barcode[1] + barcode[2] + barcode[3] + barcode[4] + barcode[5] + barcode[6]);
		RConsole.println("The Barcode is: " + barcode[0] + barcode[1] + barcode[2] + barcode[3] + barcode[4] + barcode[5] + barcode[6]);

		int[][] correctBarCodes = {{1,0,0,1,1,0,0},{1,1,0,0,1,1,0},{1,1,1,0,0,0,0},{1,1,1,1,1,1,1},{0,0,0,0,0,0,0}};

		if (arrayEquals(barcode, correctBarCodes[0]))
			master.barCodeDetected(BarCode.RECHTS);
		else if (arrayEquals(barcode, correctBarCodes[1])){
			//RConsole.println("detected");
			master.barCodeDetected(BarCode.LINKS);
		}
		else if (arrayEquals(barcode, correctBarCodes[2]))
			master.barCodeDetected(BarCode.RECHTDOOR);
		else if (arrayEquals(barcode, correctBarCodes[3])){
			//RConsole.println("detected");
			master.barCodeDetected(BarCode.ZWARTELIJN);}
		else if (arrayEquals(barcode, correctBarCodes[4])){
			//RConsole.println("detected");
			master.barCodeDetected(BarCode.WITTELIJN);}
		
		RConsole.println("Barcode verzonden");
	}
	
	private boolean arrayEquals(int[] array1, int[] array2){
		for(int i = 0; i<7; i++){
			if(array1[i]!=array2[i])
				return false;
		}
		return true;
	}
	private int[] correctBarCode(int[] barCode)
	{
		//		int[][] correctBarCodes = {{1,1,1,1,1,1,1},{1,1,1,0,0,0,0},{1,1,0,1,0,0,1},{1,1,0,0,1,1,0},{1,0,1,1,0,1,0},{1,0,1,0,1,0,1},{1,0,0,1,1,0,0},{1,0,0,0,0,1,1},{0,1,1,1,1,0,0},{0,1,1,0,0,1,1},{0,1,0,1,0,1,0},{0,1,0,0,1,0,1},{0,0,1,1,0,0,1},{0,0,1,0,1,1,0},{0,0,0,1,1,1,1},{0,0,0,0,0,0,0}};
		int[][] correctBarCodes = {{1,0,0,1,1,0,0},{1,1,0,0,1,1,0},{1,1,1,0,0,0,0},{1,1,1,1,1,1,1},{0,0,0,0,0,0,0}};
		int[] distance = new int[5];
		int smallestDistance=7;
		int smallestIndex=0;

		//bereken distance voor elke barcode
		for(int i = 0; i<5; i++){
			for(int k = 0; k<7; k++){
				distance[i] += Math.abs(correctBarCodes[i][k]-barCode[k]);
			}
		}

		//zoek index met kleinste distance
		for(int i = 0; i<5; i++){
			if(distance[i] == 0){
				//distance 0 -> kleinste distance
				smallestIndex = i;
				break;
			}
			if(distance[i]<smallestDistance){
				smallestDistance = distance[i];
				smallestIndex = i;
			}
		}

		return correctBarCodes[smallestIndex];
	}

	// zie http://en.wikipedia.org/wiki/Finite_difference_coefficients
	private int finiteDifference() {
		// voorwaartse differentie over 4 punten
		//		float difference = - 11 * signalBuffer[signalBuffer.length - 4]
		//		                   + 18 * signalBuffer[signalBuffer.length - 3]
		//		                   - 9 * signalBuffer[signalBuffer.length - 2]
		//		                   + 2 * signalBuffer[signalBuffer.length - 1];
		//		return Math.round(difference / (samplingPeriod * 6));
		// voorwaartse differentie over 3 punten
		//		float difference = - 3 * signalBuffer[signalBuffer.length - 3]
		//		                   + 4 * signalBuffer[signalBuffer.length - 2]
		//		                   - 1 * signalBuffer[signalBuffer.length - 1];
		//		return Math.round(difference / (samplingPeriod * 2));
		return signalBuffer[signalBuffer.length - 1] - signalBuffer[signalBuffer.length - 2];
	}

	private int bufferMean()
	{
		int sum = 0;  // sum of all the elements
		for (int i = 0; i < signalBuffer.length; i++) {
			sum += signalBuffer[i];
		}
		return sum / signalBuffer.length;

	}

	public void fillSignalBuffer(int value)
	{
		for (int i = 0; i < signalBuffer.length; i++) {
			signalBuffer[i] = value;
		}
	}
}